#include <sxOpenGL_PickingController.h>
#include <gl/glut.h>

Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::OpenGL_PickingController() : PickingController() {
	std::memset(this->projMatrix, 0, sizeof(float) * OpenGL_PickingController::MATRIX_SIZE);
	std::memset(this->modelMatrix, 0, sizeof(float) * OpenGL_PickingController::MATRIX_SIZE);
	std::memset(this->viewport, 0, sizeof(int) * OpenGL_PickingController::VIEWPORT_DIMENSIONS);
}

Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::~OpenGL_PickingController() {}

bool Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::construct() {
	return true;
}

bool Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::update() {
	glGetDoublev(GL_PROJECTION_MATRIX, this->projMatrix);
	glGetDoublev(GL_MODELVIEW_MATRIX, this->modelMatrix);
	glGetIntegerv(GL_VIEWPORT, this->viewport);
	return true;
}

/* Function can use the gluProject(..) function or any equivalent. */
bool Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::project(const Camera* const camera, const Eigen::Vector3f& worldPoint, Eigen::Vector2i& viewPoint) {
	double winx, winy, winz;
	int result = gluProject(worldPoint.x(), worldPoint.y(), worldPoint.z(), this->modelMatrix, this->projMatrix, this->viewport, &winx, &winy, &winz);
	if ( result == GLU_FALSE ) return false;

	viewPoint.x() = winx;
	viewPoint.y() = this->viewport[3] - winy;
	return true;
}

/* Function can use the gluUnProject(..) function or any equivalent. */
bool Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::unproject(const Camera* const camera, int x, int y, Eigen::Vector3f& origin, Eigen::Vector3f& direction) {
	Eigen::Vector3f pos, dir;
	double objX, objY, objZ;
	float posX = x;
	float posY = this->viewport[3] - y;

	//--------------------------------------------------------------------------
	// Unproject the 2D coordinate to the cameras near plane. The direction of
	// the picking ray will be determined by the projection of the camera after
	// the origin is defined.
	//--------------------------------------------------------------------------
	int result = gluUnProject(posX, posY, camera->getNearPlane(), this->modelMatrix, this->projMatrix, this->viewport, &objX, &objY, &objZ);
	if ( result == GLU_FALSE ) return false;
	pos = Eigen::Vector3f(objX, objY, objZ);

	//--------------------------------------------------------------------------
	// If the camera has a perspective projection then adjust for the
	// perspective (fov) of the camera. Otherwise the projection is orthographic
	// and the picking line will be collinear with one of the 3 major axes.
	//--------------------------------------------------------------------------
	if ( camera->getProjection() == Math::PERSPECTIVE ) {
		dir = (pos - camera->getEye()) * camera->getFOV();
		dir.normalize();
	}
	else {
		const Math::View view = camera->getView();
		dir = Eigen::Vector3f::Zero();

		if ( view == Math::TOP ) {
			dir.y() = -1.0f;
			pos.y() = camera->getEye().y();
		}
		else if ( view == Math::BOTTOM ) {
			dir.y() = 1.0f;
			pos.y() = camera->getEye().y();
		}
		else if ( view == Math::LEFT ) {
			dir.x() = -1.0f;
			pos.x() = camera->getEye().x();
		}
		else if ( view == Math::RIGHT ) {
			dir.x() = 1.0f;
			pos.x() = camera->getEye().x();
		}
		else if ( view == Math::FRONT ) {
			dir.z() = -1.0f;
			pos.z() = camera->getEye().z();
		}
		else if ( view == Math::BACK ) {
			dir.z() = 1.0f;
			pos.z() = camera->getEye().z();
		}
	}
	
	//--------------------------------------------------------------------------
	// Assign the origin and direction of the picking ray to the out params.
	//--------------------------------------------------------------------------
	origin = pos;
	direction = dir;
	return true;
}

bool Sx::OpenGL::Graphics::Picking::OpenGL_PickingController::createPickRay(const Camera* const camera, int x, int y, Eigen::Vector3f& start, Eigen::Vector3f& end) {
	Eigen::Vector3f pos, dir;
	double objX, objY, objZ;
	float posX = x;
	float posY = this->viewport[3] - y;

	//--------------------------------------------------------------------------
	// Unproject the 2D coordinate to the cameras near plane. The direction of
	// the picking ray will be determined by the projection of the camera after
	// the origin is defined.
	//--------------------------------------------------------------------------
	int result = gluUnProject(posX, posY, camera->getNearPlane(), this->modelMatrix, this->projMatrix, this->viewport, &objX, &objY, &objZ);
	if ( result == GLU_FALSE ) return false;
	pos = Eigen::Vector3f(objX, objY, objZ);

	//--------------------------------------------------------------------------
	// If the camera has a perspective projection then adjust for the
	// perspective (fov) of the camera. Otherwise the projection is orthographic
	// and the picking line will be collinear with one of the 3 major axes.
	//--------------------------------------------------------------------------
	if ( camera->getProjection() == Math::PERSPECTIVE ) {
		dir = (pos - camera->getEye()) * camera->getFOV();
		dir.normalize();
		dir *= this->extent;
	}
	else {
		const Math::View view = camera->getView();
		if ( view == Math::TOP ) {
			dir.x() = pos.x();
			dir.y() = -this->extent;
			pos.y() = this->extent;
			dir.z() = pos.z();
		}
		else if ( view == Math::BOTTOM ) {
			dir.x() = pos.x();
			dir.y() = this->extent;
			pos.y() = -this->extent;
			dir.z() = pos.z();
		}
		else if ( view == Math::LEFT ) {
			dir.x() =  -this->extent;
			pos.x() = this->extent;
			dir.y() = pos.y();
			dir.z() = pos.z();
		}
		else if ( view == Math::RIGHT ) {
			dir.x() = this->extent;
			pos.x() = -this->extent;
			dir.y() = pos.y();
			dir.z() = pos.z();
		}
		else if ( view == Math::FRONT ) {
			dir.x() = pos.x();
			dir.y() = pos.y();
			dir.z() = -this->extent;
			pos.z() = this->extent;
		}
		else if ( view == Math::BACK ) {
			dir.x() = pos.x();
			dir.y() = pos.y();
			dir.z() = this->extent;
			pos.z() = -this->extent;
		}
	}
	
	//--------------------------------------------------------------------------
	// Assign the origin and direction of the picking ray to the out params.
	//--------------------------------------------------------------------------
	start = pos;
	end = dir;
	return true;
}

